#include "EntechDriveSubsystem.h"
#include "EntechConstants.h"

EntechDriveSubsystem::EntechDriveSubsystem()
{
	m_leftJoystick = Joystick::GetStickForPort(c_joystickLeftPort);
	m_rightJoystick = Joystick::GetStickForPort(c_joystickRightPort);
	
	m_leftDriveMotor = new Talon(c_driveLeftMotor);
	m_rightDriveMotor = new Talon(c_driveRightMotor);
	
	m_leftEncoder = new Encoder(c_leftEncoderAport, c_leftEncoderBport, false);
	m_rightEncoder = new Encoder(c_rightEncoderAport, c_rightEncoderBport, false);
	m_leftEncoder->SetReverseDirection(false);
	m_rightEncoder->SetReverseDirection(true);
	m_leftEncoder->SetDistancePerPulse(c_distancePerEncoderPulse);
	m_rightEncoder->SetDistancePerPulse(c_distancePerEncoderPulse);
	m_leftEncoder->Start();
	m_rightEncoder->Start();
	
	m_driveModeSwitch = new OperatorButton(1, c_driveModeButton);
	m_automaticEncoderDrive = new OperatorButton(1, c_automaticDriveButton);
	
	m_leftFinished = true;
	m_rightFinished = true;
}

EntechDriveSubsystem::~EntechDriveSubsystem()
{
}

void EntechDriveSubsystem::DoRobotInit(void)
{
	
}

void EntechDriveSubsystem::DoAutonomousInit(void)
{
	
}

void EntechDriveSubsystem::DoTeleopInit(void)
{
	m_leftDriveMotor->Set(0);
	m_rightDriveMotor->Set(0);

	m_leftEncoder->Reset();
	m_rightEncoder->Reset();
	
	m_driveMode = true;
	m_autoDriveLock = false;
	m_leftFinished = false;
	m_rightFinished = false;
}

void EntechDriveSubsystem::DoDisabledInit(void)
{
	m_leftDriveMotor->Set(0);
	m_rightDriveMotor->Set(0);
}

void EntechDriveSubsystem::DoAutonomousPeriodic(void)
{
	
}

void EntechDriveSubsystem::DoTeleopPeriodic(void)
{
	
	if (m_driveModeSwitch->Get() == OperatorButton::kJustPressed)
	{
		m_driveMode = (m_driveMode ? false : true);
	}
	
	
	if (m_automaticEncoderDrive->Get() == OperatorButton::kJustPressed)
	{
		m_leftEncoder->Reset();
		m_rightEncoder->Reset();
		m_autoDriveLock = true;
	}
	
	if (m_autoDriveLock == false)
	{
		EntechDrive(m_leftJoystick, m_rightJoystick);
	}
	else
	{
		DriveByDistance(12, 12);
	}
	
	SmartDashboard::PutNumber("Left Encoder Distance: ", m_leftEncoder->GetDistance());
	SmartDashboard::PutNumber("Right Encoder Distance: ", m_rightEncoder->GetDistance());
	SmartDashboard::PutNumber("Left Encoder Rate: ", m_leftEncoder->GetRate());
	SmartDashboard::PutNumber("Right Encoder Rate: ", m_rightEncoder->GetRate());
}

void EntechDriveSubsystem::DoDisabledPeriodic(void)
{
	
}

void EntechDriveSubsystem::EntechDrive(Joystick *js_left, Joystick *js_right)
{
	double lmotor, rmotor;
	double jy, jx;
	
	if (m_driveMode == true)
	{
		// Tank Drive Logic
		lmotor = js_left->GetY();
		rmotor = js_right->GetY();
	}
	else
	{
		// Arcade Drive (Un-)Logic
		jy = js_left->GetY();
		jx = js_left->GetX();
		if (jy > 0.0) 
		{
			if (jx > 0.0)
			{
				lmotor = jy - jx;
				rmotor = max(jy, jx);
			}
			else
			{
				lmotor = max(jy, -jx);
				rmotor = jy + jx;
			}
		}
		else
		{
			if (jx > 0.0)
			{
				lmotor = -max(-jy, jx);
				rmotor = jy + jx;
			}
			else
			{
				lmotor = jy - jx;
				rmotor = -max(-jy, -jx);
			}
		}
	}
	
	SetDriveMotors(lmotor, rmotor);
}

void EntechDriveSubsystem::SetDriveMotors(double lmotor, double rmotor)
{
	//double x = 0;
	
	lmotor = min(1.0,max(-1.0,lmotor));
	rmotor = min(1.0,max(-1.0,rmotor));
	
	/*
	if (m_leftEncoder->GetRate() - c_encoderDeadzone > m_rightEncoder->GetRate() || m_rightEncoder->GetRate() - c_encoderDeadzone > m_leftEncoder->GetRate())
	{
		if (m_leftEncoder->GetRate() > m_rightEncoder->GetRate())
		{
			
		}
	}
	*/
	
	m_leftDriveMotor->Set(-1*lmotor);
	m_rightDriveMotor->Set(rmotor);
}
void EntechDriveSubsystem::DriveByDistanceInInches(double leftInches, double rightInches)
{
	DriveByDistance(leftInches, rightInches);
}
bool EntechDriveSubsystem::IsAutoDriveFinished(void)
{
	return (m_leftFinished && m_rightFinished);
}

void EntechDriveSubsystem::DriveByDistance(double leftEncoderTarget, double rightEncoderTarget)
{
	if(m_leftFinished == false || m_rightFinished == false)
	{
		if(m_leftEncoder->GetDistance() < leftEncoderTarget)
		{
			m_leftDriveMotor->Set(.5);
		}	
		else
		{
			m_leftDriveMotor->Set(0);
			m_leftFinished = true;
		}
		if(m_rightEncoder->GetDistance() < rightEncoderTarget)
		{
			m_rightDriveMotor->Set(-.5);
		}
		else
		{
			m_rightDriveMotor->Set(0);
			m_rightFinished = true;
		}
	}
	else
	{
		m_leftEncoder->Reset();
		m_rightEncoder->Reset();
		m_autoDriveLock = false;
		m_leftFinished = false;
		m_rightFinished = false;
	}
}
